#include "EvolutiveSimulationThread.h"

namespace jcrada {

    EvolutiveSimulationThread::EvolutiveSimulationThread(BulletSimulator& simulator) :
    SimulationThread(simulator),
    _evolution(NULL),
    _seconds_for_stabilization(3) {

    }
    EvolutiveSimulationThread::~EvolutiveSimulationThread(){
        if (_evolution){
            delete _evolution;
        }
    }

    void EvolutiveSimulationThread::stabilize(Creature& creature) {
        btVector3 axis(0.0f, 1.0f, 0.0f); // pointing upwards, aka Y-axis
        btHingeConstraint* constraint = new btHingeConstraint(
                *creature.getRoot().getRigidBody(),
                btVector3(0, 0.0, 0.0f), axis);
        constraint->setLimit(0.0f, 0.0);
        getSimulator().getWorld().addConstraint(constraint, true);
        bool was_behavior_enabled = isBehaviorEnabled();
        setBehaviorEnabled(false);
        int simulation_steps = getSecondsForStabilization() * getSimulator().getTimestepsPerSecond();
        for (int i = 0; i < simulation_steps; ++i) {
            getTimer().restart();
            performStep();
            ensureFrameRate(getTimer().elapsed());
        }
        setBehaviorEnabled(was_behavior_enabled);
        getSimulator().getWorld().removeConstraint(constraint);
        delete constraint;
    }

    void EvolutiveSimulationThread::iterate() {
        if (!getEvolution()) {
            emit message("Evolution not set");
            setStopped(true);
            return;
        }
        int simulation_steps = getEvolution()->getEvaluationTime() *
                getSimulator().getTimestepsPerSecond();
        Creature* creature = NULL;
        getEvolution()->prepareCreatures();
        double fitness = 0.0;
        for (int i = 0; i < getEvolution()->getPopulationSize(); ++i) {
            creature = &getEvolution()->getCreature(i);
            getSimulator().reset();
            getSimulator().addCreature(*creature);
            stabilize(*creature);
            for (int j = 0; j < simulation_steps; ++j) {
                getTimer().restart();
                performStep();
                ensureFrameRate(getTimer().elapsed());
            }
            getSimulator().removeCreature();
            creature->setFitness((creature->getFinalPosition() - creature->getInitialPosition()).length());
            fitness += creature->getFitness();
        }
        std::string m = TO_STRING(getCurrentIteration()) +
                "\t" + TO_STRING(fitness / getEvolution()->getPopulationSize());
        emit message(QString(m.c_str()));
        getEvolution()->step();
        setCurrentIteration(getCurrentIteration() + 1);
    }
}
